#ifndef _FACEDIR_H_
#define _FACEDIR_H_

#include <iostream>
#include "SMATModel.hh"
#include "display.hh"
#include "utils.hh"
#include <string.h>

#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <semaphore.h>

#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>
#include "smat-ransac.h"


#include "videoAVI.h"
#include "constants.h"
#include "face_detect_vj.hh"
#include "modelo_RANSAC.h"
#include "read_video.h"
#include "other_functions.h"
#include "options.h"
#include <colores.h>


class CFacedir {
 public:
    boost::shared_ptr<CvVideoWriter> video_results_SMAT, video_results_FaceDir;
    boost::shared_ptr<CvFileStorage> pose_results;
    boost::shared_ptr<IplImage> srcD, srcI;
    CALIBRATION_PARAMS_t calibration_paramsL, calibration_paramsR;



    CvMat *points3D, *pointsID[2];
    CvMat *angY;
    CvMat * chesscorners;
    IplImage *image2[2];
    CvMat * lines;
    CvMat *epilines;
    CvMat *rot;
    CvMat * mask_show;
    int npoints;

    int graficar;
    int groundtruth;
    /***********/
    //lanza el hilo de lectura de las imágenes
    video_params_t video_params;
    int iniframe;
    stereo_IplImage_t *imgs;
    int wait;

    IplImage *clonek, *display;
    IplImage * buffer_show_imgs[SHOW_IMAGE_BUFFER_SIZE];
    IplImage * buffer_display_smat[SHOW_IMAGE_BUFFER_SIZE];
    int np_to_show_imgs, np_to_show_imgs2;

    FILE * filedata;
    int lastframe;

    boost::shared_ptr<SMATModel> smatI;
    boost::shared_ptr<SMATModel> smatD; 

    FILE * fout ;
    int save_video;
    int save_pose;
    FILE * videofile;

   //  leer el modelo de un archivo
    //inicializar cosas de Pedro
    CvMat *vectL, *vect_media_L, *translationL, *translation_media_L, *vectRl, *translationRl;
    CvMat *vectR, *vect_media_R, *translationR, *translation_media_R;
    CvMat *rotation;
    CvMat * offsettrans, *offsettrans2;
    float *x2, *y2, *z2;
    int inicio_frame;

    std::vector<boost::shared_ptr<CvMat> > smat_pts;
    IplImage *img[2], *img_smat[2];

    CvScalar offsetmodel;
    
    CvMat pointsID2[2];
    CvScalar desplazar_parche;

   
    CvMat *maskL;
    CvMat *maskR;
    CvMat *maskoutI;
    CvMat *maskoutD; 
    CvMat *maskoutI2;
    CvMat *maskoutD2;

    CvMat *maskoutL;
    CvMat *maskoutL2;
    CvMat *masktemp;

    CvMat *points2D_reprojectI;
    CvMat *points2D_reprojectD;
    //CvMat col, submat;
    int inliersI, inliersD;
    //int outlierI, outliersD;

    unsigned int frame;
    bool quit;  

    thread_params_t params_left, params_right;
    sem_t sem1, sem2;


    int key;

    CvMat * offsetvect;
    float ai, bi, gi;
    CvMat * pointsID_old[2];
    int modificado, modificando;
    float incremento;
    float incrementopos;
    float *a, *b, *c, *x, *y, *z;
    float *a2, *b2, *c2;
    CvMat * vect_2;
    int frame2;
    CvMat * oldtrans;
    CvMat * oldvect;
    double disttrans;    

 public:
    int main(void);
    void facedir(void );
    void facedir_control(void);
};


int bucle_cvWaitKey(int * wait);

#endif
